﻿using System.Diagnostics;
using RaytracerLib.MathLib;
using YAXLib;
using RaytracerLib.Rays;
using System;

namespace RaytracerLib
{
    public abstract class OrientedElement : SceneActor
    {
        internal protected Matrix4 LocalToWorld { get; protected set; }
        internal protected Matrix4 WorldToLocal { get; protected set; }
        internal protected Matrix4 LocalToWorldNormal { get; protected set; }
        internal protected Matrix4 WorldToLocalNormal { get; protected set; }
        internal protected Matrix4 WorldToLocalRotation { get; protected set; }

        [YAXNode("Right")]
        private Vector3 m_right;

        [YAXNode("Forward")]
        private Vector3 m_forward;

        [YAXNode("Up")]
        private Vector3 m_up;

        [YAXNode("Scale")]
        private Vector3 m_scale;

        protected OrientedElement(Vector3 a_right, Vector3 a_up)
        {
            Debug.Assert(!a_right.IsAlmostRelativeEquals(a_up));

            m_right = a_right.Normalized;
            m_up = a_up.Normalized;
            m_forward = Vector3.CrossProduct(m_right, m_up);
            m_scale = Vector3.ONE;
        }

        protected OrientedElement(Vector3 a_right, Vector3 a_up, Vector3 a_forward)
        {
            Debug.Assert(!a_right.IsAlmostRelativeEquals(a_up));
            Debug.Assert(a_right.IsAlmostRelativeEquals(a_forward));
            Debug.Assert(a_up.IsAlmostRelativeEquals(a_forward));

            m_right = a_right.Normalized;
            m_up = a_up.Normalized;
            m_forward = a_forward.Normalized;
            m_scale = Vector3.ONE;

            Update(UpdateFlags.All);
        }

        protected virtual Vector3 LocalScale
        {
            get
            {
                return Vector3.ONE;
            }
        }

        protected virtual Matrix4 GetLocalToWorldMatrix()
        {
            return Matrix4.CreateTranslation(Pos) * 
                   new Matrix4(Right, Up, Forward) * 
                   Matrix4.CreateScale(Scale.Scale(LocalScale));
        }

        protected override void Update(UpdateFlags a_flags)
        {
            if (a_flags.HasFlag(UpdateFlags.Matrices))
            {
                LocalToWorld = GetLocalToWorldMatrix();
                WorldToLocal = LocalToWorld.Inverted;
                LocalToWorldNormal = WorldToLocal.Transposed.Rotation;
                WorldToLocalNormal = LocalToWorldNormal.Inverted;
                WorldToLocalRotation = WorldToLocal.Rotation;
            }

            base.Update(a_flags);
        }

        public virtual Vector3 Scale
        {
            get
            {
                return m_scale;
            }
            set
            {
                m_scale = value;

                Update(UpdateFlags.BoundBox | UpdateFlags.Matrices);
            }
        }

        public double ScaleX
        {
            get
            {
                return m_scale.X;
            }
            set
            {
                Scale = new Vector3(value, Scale.Y, Scale.Z);
            }
        }

        public double ScaleY
        {
            get
            {
                return m_scale.Y;
            }
            set
            {
                Scale = new Vector3(Scale.X, value, Scale.Z);
            }
        }

        public double ScaleZ
        {
            get
            {
                return m_scale.Z;
            }
            set
            {
                Scale = new Vector3(Scale.X, Scale.Y, value);
            }
        }

        public Vector3 Up
        {
            get
            {
                return m_up;
            }
            set
            {
                m_up = value.Normalized;
                Update(UpdateFlags.BoundBox | UpdateFlags.Matrices);
            }
        }

        public Vector3 Right
        {
            get
            {
                return m_right;
            }
            set
            {
                m_right = value.Normalized;
                Update(UpdateFlags.BoundBox | UpdateFlags.Matrices);
            }
        }

        public Vector3 Forward
        {
            get
            {
                return m_forward;
            }
            set
            {
                m_forward = value.Normalized;
                Update(UpdateFlags.BoundBox | UpdateFlags.Matrices);
            }
        }

        /// <summary>
        /// Angle in deg.
        /// Yawn.
        /// </summary>
        /// <param name="a_angle"></param>
        public void RotateAroundRight(double a_angle)
        {
            RotateAround(Pos, Right, a_angle);
        }

        /// <summary>
        /// Angle in deg.
        /// Pitch.
        /// </summary>
        /// <param name="a_angle"></param>
        public void RotateAroundUp(double a_angle)
        {
            RotateAround(Pos, Up, a_angle);
        }

        /// <summary>
        /// Angle in deg.
        /// Roll.
        /// </summary>
        /// <param name="a_angle"></param>
        public void RotateAroundForward(double a_angle)
        {
            RotateAround(Pos, Forward, a_angle);
        }

        /// <summary>
        /// Angle in deg.
        /// </summary>
        /// <param name="a_point"></param>
        /// <param name="a_vector"></param>
        /// <param name="a_angle"></param>
        public override void RotateAround(Vector3 a_point, Vector3 a_vector, double a_angle)
        {
            base.RotateAround(a_point, a_vector, a_angle);

            Matrix4 t = Matrix4.CreateRotateAroundVector(a_vector, a_angle).Rotation;

            m_right = (t * m_right).Normalized;
            m_up = (t * m_up).Normalized;
            m_forward = (t * m_forward).Normalized;

            Update(UpdateFlags.All);
        }

        public override void Transform(Matrix4 a_matrix)
        {
            base.Transform(a_matrix);

            Matrix4 rotation = a_matrix.Rotation;

            m_right = (rotation * m_right).Normalized;
            m_up = (rotation * m_up).Normalized;
            m_forward = (rotation * m_forward).Normalized;

            Update(UpdateFlags.All);
        }

        protected override AABB GetBoundBox()
        {
            AABB local_aabb = GetLocalBoundBox();

            Vector3 min = local_aabb.Minimum;
            Vector3 max = local_aabb.Maximum;

            Vector3 v1 = LocalToWorld * new Vector3(min.X, max.Y, max.Z);
            Vector3 v2 = LocalToWorld * new Vector3(min.X, max.Y, min.Z);
            Vector3 v3 = LocalToWorld * new Vector3(min.X, min.Y, max.Z);
            Vector3 v4 = LocalToWorld * new Vector3(min.X, min.Y, min.Z);
            Vector3 v5 = LocalToWorld * new Vector3(max.X, max.Y, max.Z);
            Vector3 v6 = LocalToWorld * new Vector3(max.X, max.Y, min.Z);
            Vector3 v7 = LocalToWorld * new Vector3(max.X, min.Y, max.Z);
            Vector3 v8 = LocalToWorld * new Vector3(max.X, min.Y, min.Z);

            return new AABB(v1, v2, v3, v4, v5, v6, v7, v8);
        }

        protected void TransformToLocal(Ray a_ray, out Vector3 a_local_dir, out Vector3 a_local_start)
        {
            a_local_dir = (WorldToLocalRotation * a_ray.Dir).Normalized;
            a_local_start = WorldToLocal * a_ray.Start;
        }

        protected bool TransformToLocalToBoundBox(Ray a_ray, out Vector3 a_local_dir,
            out Vector3 a_local_start, out double a_near, out double a_far, 
            double a_margin = Constants.BOUND_BOX_MARGIN)
        {
            if (!BoundBox.GetIntersections(a_ray, out a_near, out a_far))
            {
                a_local_dir = default(Vector3);
                a_local_start = default(Vector3);
                a_near = Double.PositiveInfinity;
                a_far = Double.PositiveInfinity;
                return false;
            }

            Vector3 world_near_hit = a_ray.Start + a_ray.Dir * a_near;
            Vector3 world_far_hit = a_ray.Start + a_ray.Dir * a_far;
            Vector3 local_near_hit = WorldToLocal * world_near_hit;
            Vector3 local_far_hit = WorldToLocal * world_far_hit;

            a_local_dir = (WorldToLocalRotation * a_ray.Dir).Normalized;
            a_local_start = WorldToLocal * a_ray.Start;

            a_near = Math.Sign(a_near) * (local_near_hit - a_local_start).Length;
            a_far = Math.Sign(a_far) * (local_far_hit - a_local_start).Length;

            if (a_near > a_margin)
            {
                a_far = (a_far - a_near) + a_margin;
                a_near = 0;
                a_local_start = local_near_hit - a_local_dir * a_margin;
            }

            return true;
        }

        protected bool TransformToLocalToBoundBox(Ray a_ray, out Vector3 a_local_dir,
            out Vector3 a_local_start, double a_margin = Constants.BOUND_BOX_MARGIN)
        {
            double near, far;
            return TransformToLocalToBoundBox(a_ray, out a_local_dir, out a_local_start, 
                out near, out far, a_margin);
        }
    }
}